#include "imu_quat.h"
#include "math.h"
#include "bsp_usart.h"
#include "filter.h"

/* 单位转换系数 */
#define G           9.80665f            // m/s^2
#define DEG2RAD     0.01745329f         // pi/180
#define RAD2DEG     57.2957795f         // 180/pi

/* PI补偿参数 */
#define Kp          (1.50f)
#define Ki          (0.005f)

// 对加速度进行卡尔曼滤波的初始参数
KalmanFilter_t EKF[3] = {
    {0.02, 0, 0, 0, 0.001, 0.543},
    {0.02, 0, 0, 0, 0.001, 0.543},
    {0.02, 0, 0, 0, 0.001, 0.543}
};

/**
 * @brief 快速计算 1/Sqrt(x)
 * 
 * @param x 
 * @return float 
 */
static float invSqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;
    i = 0x5f3759df - (i>>1);
    y = *(float*)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

/**
 * @brief 获取加速度和角速度数据并滤波
 * 
 * @param MPU6050 
 */
void IMU_GetData(MPU6050_t *MPU6050)
{
    float gxLast = MPU6050->gx;
    float gyLast = MPU6050->gy;
    float gzLast = MPU6050->gz;

    /* 获取数据 */
    MPU_GetData(MPU6050);

    /* 对加速度计滤波 */
    MPU6050->ax = KalmanFilter(&EKF[0], (float)MPU6050->ax);
    MPU6050->ay = KalmanFilter(&EKF[1], (float)MPU6050->ay);
    MPU6050->az = KalmanFilter(&EKF[2], (float)MPU6050->az);

    /* 对陀螺仪角速度滤波 */
    MPU6050->gx = Complementary_Filter(MPU6050->gx, gxLast, 0.98f);
    MPU6050->gy = Complementary_Filter(MPU6050->gy, gyLast, 0.98f);
    MPU6050->gz = Complementary_Filter(MPU6050->gz, gzLast, 0.98f);
    // printf("%.2f,%.2f,%.2f\n", MPU6050->ax, MPU6050->ay, MPU6050->az);

    // Mahony互补滤波
    // IMU_Update(angle);
}

/**
 * @brief 
 * 
 * @param MPU6050 MPU6050的数据
 * @param angle 欧拉角
 * @param dt 计算周期，单位s
 * @return uint8_t 1 -- 正常        
 *                 0 -- 异常：ax*ay*az == 0
 */
uint8_t IMU_Update(MPU6050_t *MPU6050, EulerAngle_t *angle, float dt)
{
    static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;		//四元数
    static float exInt = 0, eyInt = 0, ezInt = 0;		//叉积计算误差的累计积分
    float ax, ay, az;
    float gx, gy, gz;

    u8 i;
    float halfDt = dt / 2.0;
    float vx, vy, vz;							//实际重力加速度
    float ex, ey, ez;							//叉积计算的误差
    float norm;

    float q0q0 = q0*q0;
    float q0q1 = q0*q1;
    float q0q2 = q0*q2;
    float q0q3 = q0*q3;
    float q1q1 = q1*q1;
    float q1q2 = q1*q2;
    float q1q3 = q1*q3;
    float q2q2 = q2*q2;
    float q2q3 = q2*q3;
    float q3q3 = q3*q3;
    ax = MPU6050->ax;
    ay = MPU6050->ay;
    az = MPU6050->az;
    gx = MPU6050->gx;
    gy = MPU6050->gy;
    gz = MPU6050->gz;
    
    if (ax*ay*az == 0)
        return 0;
    
    //加速度计测量的重力方向(机体坐标系)：归一化
    norm = invSqrt(ax*ax + ay*ay + az*az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;

    if (norm < 1e-6f)  // 检查向量模长是否接近0
        return 0;

    //四元数推出的实际重力方向(机体坐标系)
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    //叉积误差
    ex = (ay*vz - az*vy);
    ey = (az*vx - ax*vz);
    ez = (ax*vy - ay*vx);

    //叉积误差积分为角速度
    exInt = exInt + ex * Ki * dt;
    eyInt = eyInt + ey * Ki * dt;
    ezInt = ezInt + ez * Ki * dt;

    //角速度补偿
    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;

    //更新四元数
    float q0Last = q0;
    float q1Last = q1;
    float q2Last = q2;
    float q3Last = q3;
    q0 = q0 + (-q1Last*gx - q2Last*gy - q3Last*gz) * halfDt;
    q1 = q1 + ( q0Last*gx + q2Last*gz - q3Last*gy) * halfDt;
    q2 = q2 + ( q0Last*gy - q1Last*gz + q3Last*gx) * halfDt;
    q3 = q3 + ( q0Last*gz + q1Last*gy - q2Last*gx) * halfDt;

    //单位化四元数
    norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0 * norm;
    q1 = q1 * norm;
    q2 = q2 * norm;
    q3 = q3 * norm;

    //四元数反解欧拉角：单位为度
    angle->roll = atan2f(2.0f * (q2*q3 + q0*q1), q0*q0 - q1*q1 - q2*q2 + q3*q3) * RAD2DEG;
    angle->pitch = -asinf(2.0f * (q1*q3 - q0*q2)) * RAD2DEG;
    angle->yaw = 2*atan2f(2.0f * (q1*q2 + q0*q3), q0*q0 + q1*q1 - q2*q2 - q3*q3) * RAD2DEG;

    /* yaw零漂处理 */
    // static float k = -0.002433, b = -0.064403, t = 0;
    // t += dt;
    // float yawError = k * t + b;
    // angle->yaw -= yawError;

    return 1;
}

